#ifndef DISTANCE_H
#define DISTANCE_H

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <std_msgs/ColorRGBA.h>
#include <string> 
#include "transform.h"
#include "map.hpp"
//计算车辆和车道线的最短距离

class CalDistance{
public:
    CalDistance(ros::NodeHandle& n)
    {
        nh = n;
        std::string odom_topic,search_topic;
        nh.getParam("odom_topic",odom_topic);
        nh.getParam("car_info/width",car_width_);
        nh.getParam("car_info/length",car_length_);
        nh.getParam("search_topic",search_topic);
        sample_car();
        odom_sub = nh.subscribe(odom_topic,5,&CalDistance::odom_callback,this);
        search_sub = nh.subscribe(search_topic,5,&CalDistance::seach_callback,this);
        bound_vis_pub = nh.advertise<visualization_msgs::Marker>("car/bound",5);
        std::string map_topic;
        custom_map_ptr_.reset(new CustomMap(n,map_topic));
    }


    double get_min_distance(Eigen::Matrix4d& car2map)
    {
        double min_distance = DBL_MAX;
        for(int i=0;i<sample_point.size();i++)
        {
             auto p_inmap = car2map * sample_point[i];
            double cur_dis = custom_map_ptr_->get_distance(p_inmap(0),p_inmap(1));
            min_distance = std::min(cur_dis,min_distance);
        }
        return min_distance;
    }


private:
    void odom_callback(const nav_msgs::OdometryConstPtr& msg)
    {
        auto odom_msg = *msg;
        Eigen::Matrix4d car2map;
        transform::Pose2Mat4d(car2map,odom_msg.pose.pose);
        update_bound(car2map);
        visualize_bound();
        
    }

    void seach_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
    {
        auto pose = msg->pose.pose;
        Eigen::Matrix4d mat;
        transform::Pose2Mat4d(mat,pose);
        std::cout<<"min_distance:"<<get_min_distance(mat)<<std::endl;;
    }

    void sample_car()
    {
        //7点采样
        Eigen::Vector4d left_front(car_length_/2,-car_width_/2,0,1);
        Eigen::Vector4d front(car_length_/2,0,0,1);
        Eigen::Vector4d right_front(car_length_/2,car_width_/2,0,1);
        Eigen::Vector4d right(0,car_width_/2,0,1);
        Eigen::Vector4d right_rear(-car_length_/2,car_width_/2,0,1);
        Eigen::Vector4d left_rear(-car_length_/2,-car_width_/2,0,1);
        Eigen::Vector4d left(0,-car_width_/2,0,1);

        sample_point.emplace_back(left_front);
        sample_point.emplace_back(front);
        sample_point.emplace_back(right_front);
        sample_point.emplace_back(right);
        sample_point.emplace_back(right_rear);
        sample_point.emplace_back(left_rear);
        sample_point.emplace_back(left);
        car_bound_inmap = std::vector<geometry_msgs::Point>(sample_point.size(),geometry_msgs::Point());
    }

    void update_bound(Eigen::Matrix4d& car2map)
    {
        for(int i=0;i<sample_point.size();i++)
        {
             auto p_inmap = car2map * sample_point[i];
             car_bound_inmap[i].x  =p_inmap(0);
             car_bound_inmap[i].y = p_inmap(1);
             car_bound_inmap[i].z = p_inmap(2);
        }
    }

    void visualize_bound()   //可视化车辆边界
    {
        std_msgs::ColorRGBA color;
        color.r = 0.0;
        color.g = 1.0;
        color.b = 0.0;
        color.a = 1.0;
        visualization_msgs::Marker bound;
        bound.header.frame_id = "/map";
        bound.type = visualization_msgs::Marker::LINE_STRIP;
        bound.action = visualization_msgs::Marker::ADD;
        bound.scale.x = 0.05;


        for(int i=0;i<car_bound_inmap.size();i++)
        {
            bound.points.emplace_back(car_bound_inmap[i]);
            bound.colors.emplace_back(color);
        }

        bound.points.emplace_back(car_bound_inmap[0]);
        bound.colors.emplace_back(color);
        bound_vis_pub.publish(bound);
    }


private:
    std::vector<Eigen::Vector4d> sample_point;     //对车身进行采样计算到车道线的最短距离
    std::vector<geometry_msgs::Point> car_bound_inmap;






private:
    ros::NodeHandle nh;
    ros::Subscriber odom_sub;
    ros::Subscriber search_sub;
    ros::Publisher bound_vis_pub;
    double car_length_;
    double car_width_;
    std::shared_ptr<CustomMap> custom_map_ptr_;

};



#endif